#include "tankmessage.h"

module DebugMessageDispatcherP {

	uses {
		interface Boot;
		interface SplitControl as AMControl;

//		interface Receive as ReceiveDebugControl;

		interface AMSend as SendDebugInfo;

		interface Packet;

		interface MainMotorDebug;

	}
}

implementation {
	message_t sendbuffer;

	event void Boot.booted() {
		call AMControl.start();
	}

	event void AMControl.startDone(error_t err) {
		if (err == SUCCESS) {
			// t_state = 1;

		}
		else {
			call AMControl.start();
		}
	}

	event void AMControl.stopDone(error_t err) {
		// do nothing
	}
/*	
	event message_t* ReceiveDebugControl.receive(message_t* bufPtr, void* payload, uint8_t len) {
		if (len == sizeof(tank_move_msg_t)) {
			tank_move_msg_t * msgdata = (tank_move_msg_t *)payload;
		}
		return bufPtr;
	}
*/

	event void MainMotorDebug.controllerTick(uint16_t csleft, uint16_t csright, int16_t Ileft, int16_t Iright, int16_t pwmleft, int16_t pwmright) {
		tank_debug_pid_msg_t *d=(tank_debug_pid_msg_t *)sendbuffer.data;
		d->csleft = csleft;
		d->csright = csright;
		d->Ileft = Ileft;
		d->Iright = Iright;
		d->pwmleft = pwmleft;
		d->pwmright = pwmright;
		call SendDebugInfo.send(0xFFFF, &sendbuffer, sizeof(tank_debug_pid_msg_t) );
	}


	event void SendDebugInfo.sendDone(message_t* bufPtr, error_t error) {

	}

}
